//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <ddk.h>
#include <bulverde.h>
#include <drvpub.h>
#include "display_def.h"


////////////////////////////////////////////////////////////////////////////
// Frequency changing code
#define BASE_FREQ   13000000
EXTERN_C int get_cpu_mode();
static int get_cpu_speed()
{
    int speed, N2, L;
    unsigned long cccr = CCCR;
    int mode = get_cpu_mode();

    N2 = (cccr & CCCR_2N_MASK) >> 7/*CCCR_2N_SHIFT*/;
    L = (cccr & CCCR_L_MASK);
    if(N2 < 2)
        N2 = 2;
    if(L < 2)
        L = 2;

    speed = BASE_FREQ * L;
    if(mode & CLKCFG_HT)
        speed = (speed * N2) / 4;
    else if(mode & CLKCFG_T)
        speed = (speed * N2) / 2;

    return speed;
}

#define 	VOLT_MIN	0x0
#define 	VOLT_LOW	0x1
#define	VOLT_HIGH	0x2
#define	VOLT_MAX	0x3

struct core_freq{
	unsigned int frequency;
	unsigned int mHZ;
	unsigned int L;
	unsigned int twoN;
	unsigned int turbo;
	unsigned int b;
	unsigned int volt;
};
EXTERN_C int WriteCLKCFG(int);
struct core_freq cpu_freq_volt []= {
	{13, 13, 1, 2, 1, 1, VOLT_MIN},
	{104, 104, 8, 2, 1, 1, VOLT_LOW},
	//{156, 8, 6, 1, 1, VOLT_LOW},
	{208, 208, 16, 2, 1, 1, VOLT_HIGH},
	//{104, 8, 6, 1, 1, VOLT_HIGH }, //312/104/104/104
	{312, 208, 16, 3, 1, 1, VOLT_MAX}, //312/208/208/104
	{416, 208, 16, 4, 1, 1, VOLT_MAX}, //416/208/208/104
	{91, 0, 7, 2, 0, 0, VOLT_LOW} //91MHz not used in normal mode
};

void ChangeFreq(struct core_freq freq){
		int value;
		unsigned long mdrefr_val, /*cccr_val = 0, clkcfg_val = 0,*/ msc0_val, mdrefr_dri = 0;
		unsigned long L, /*M = 0,*/ N2, memclk;

		value = CCCR;
		value &= 0x3FFFFFFF; 				//make sure the Core and Peripheral PLL disable bits are clear.
		value = value & 0xFFFFF000;			//clear the L and 2N bits

		L = freq.L & 0x1F;						//make sure L is only 0 thru 31
		value |= L; 						//OR in the L value

		N2 = freq.twoN & 0xf;					//make sure 2N is only 0 thru 15

		value |= (N2 << 7);				//OR in the 2N value
		if(freq.frequency == 91)
			value &= ~CCCR_A;
		else
			value |=CCCR_A;


		/* default setting */
		msc0_val 	= MSC0;
		mdrefr_val	= MDREFR;


		/* MEM clock frequency = System Bus Frequence */
		if (freq.b)
			memclk = (L*13);
		else
			memclk = (L*13)/2;

		if(freq.frequency == 91)
			memclk = 91;


		if (memclk <= 52) {
		/* SDCLK0 = MEMCLK */
			mdrefr_val &= ~(MDREFR_K0DB2 | MDREFR_K0DB4);
		}
		else if (memclk == 91)
		{
			mdrefr_val |= MDREFR_K0DB4;
			mdrefr_val |= MDREFR_K0DB2;
			mdrefr_val |= MDREFR_K1DB2;
		}
		else if (memclk == 104) {
			mdrefr_val &= ~MDREFR_K0DB4;
			mdrefr_val |= MDREFR_K0DB2;
			mdrefr_dri = mdrefr_val;
			mdrefr_val |= 0xfff;
		}else{
			/* SDCLK0 = MEMCLK/4 */
			mdrefr_val |= MDREFR_K0DB4;

			/* SDCLK1 = MEMCLK/2 - forced */
			mdrefr_val |= MDREFR_K1DB2;
		}

		/* adjust MSC0 for bank 0 */

		msc0_val &= ~(0x7f00);
		msc0_val |= 0x5a00;


		MSC0 = msc0_val;
		(void) MSC0;

		MDREFR = mdrefr_val;
		(void) MDREFR;
		MDREFR = (MDREFR & 0x3FDFEFFF);
		(void) MDREFR;

		CCCR = value;					//now set the CCCR register
		(void *)CCCR;
		if ((freq.turbo==0) && (freq.b==0)){
    		//asm("mov	r0, #0x2");   	     		//set to Frequency Change Mode
    		//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
    		WriteCLKCFG(0x2);
		}

		if((freq.turbo==0)&&(freq.b!=0)){
		    //asm("mov	r0, #0xA");   	     		//set to Frequency Change Mode
    		//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
    		WriteCLKCFG(0xA);
		}

		if ((freq.turbo!=0) && (freq.b==0)){
    		//asm("mov	r0, #0x3");   	     		//set to Frequency Change Mode
    		//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
    		WriteCLKCFG(0x3);
		}

		if ((freq.turbo!=0) && (freq.b!=0)){
    		//asm("mov	r0, #0xB");   	     		//set to Frequency Change Mode
    		//asm("mcr	p14, 0, r0, c6, c0, 0");	//write to the CLKCFG coprocessor reg
    		WriteCLKCFG(0xB);
		}
		DzDelay(1000); //wait for the voltage to be stable
		if(memclk == 104)
		{
			mdrefr_val = mdrefr_dri;
			mdrefr_val &= ~MDREFR_K1DB2;
			MDREFR = mdrefr_val;
			(void) MDREFR;
			MDREFR = (MDREFR & 0x3FDFEFFF);
			(void) MDREFR;
		}
		else if(freq.frequency != 91)
		{
			MDREFR = mdrefr_val;
			(void) MDREFR;
			MDREFR = (MDREFR & 0x3FDFEFFF);
			(void) MDREFR;
		}

		DzDelay(1000); //wait for the voltage to be stable
}
////////////////////////////////////////////////////////////////////////////

static void LcdParamInit(DisplayContext *pCxt)
{
    pCxt->basePlaneFormat = BASEPLANE_COLORFORMAT;
    pCxt->vaBasePlane = NULL;
    pCxt->overlay[1].Enable = FALSE;
    pCxt->overlay[2].Enable = FALSE;
}

static void SetDisplayDesc(DisplayContext *pCxt)
{
    // Alloc Buffer for all channel DMA descriptors, 1 page OK.
    pCxt->vaChan0Desc = (UINT)Drv_AllocBuffer(1, 0);
    pCxt->paChan0Desc =
        Drv_GetBufferPhysicalAddress((void *)pCxt->vaChan0Desc);

    pCxt->vaChan1Desc = pCxt->vaChan0Desc  + DESC_SIZE;
    pCxt->vaChan2Desc = pCxt->vaChan1Desc  + DESC_SIZE;
    pCxt->vaChan3Desc = pCxt->vaChan2Desc  + DESC_SIZE;
    pCxt->vaChan4Desc = pCxt->vaChan3Desc  + DESC_SIZE;
    pCxt->vaChan1AltDesc = pCxt->vaChan4Desc + DESC_SIZE;
    pCxt->vaPaletteDesc = pCxt->vaChan1AltDesc + DESC_SIZE;

    pCxt->paChan1Desc = pCxt->paChan0Desc  + DESC_SIZE;
    pCxt->paChan2Desc = pCxt->paChan1Desc  + DESC_SIZE;
    pCxt->paChan3Desc = pCxt->paChan2Desc  + DESC_SIZE;
    pCxt->paChan4Desc = pCxt->paChan3Desc  + DESC_SIZE;
    pCxt->paChan1AltDesc = pCxt->paChan4Desc + DESC_SIZE;
    pCxt->paPaletteDesc = pCxt->paChan1AltDesc + DESC_SIZE;
}

static BOOL SetBaseplaneBuffer(DisplayContext *pCxt, UINT flag)
{
    // Alloc Buffer for baseplane framebuffer
    pCxt->vaBasePlane =
        (unsigned int)Drv_ReallocBuffer(
        (void *)pCxt->vaBasePlane,
        BASEPLANE_FB_PAGES, BASEPLANE_FB_PAGES, flag);
    if (!pCxt->vaBasePlane) {
        kprintf("Alloc baseplane buffer error!\n");
        return FALSE;
    }

    pCxt->paBasePlane =
        Drv_GetBufferPhysicalAddress((void *)pCxt->vaBasePlane);

    return TRUE;
}

static void ClearBaseplane(DisplayContext *pCxt)
{
    memset((void *)pCxt->vaBasePlane, 0x0, BASEPLANE_FB_SIZE);
}

static void GetOverlayInfo(DisplayContext *pCxt, int layer)
{
    assert(layer == 2);
    OverlayFormat *pOverlay = &pCxt->overlay[layer];

    // Overlay2 framebuffer address used in kernel
    pOverlay->pChannelFb[0] = (void*)pCxt->vaOverlay2Y;
    pOverlay->pChannelFb[1] = (void*)pCxt->vaOverlay2Cb;
    pOverlay->pChannelFb[2] = (void*)pCxt->vaOverlay2Cr;


    pOverlay->ChannelSize[0] = pOverlay->Width * pOverlay->Height;
    switch (pOverlay->Color) {
	    case COLOR_YCbCr422:
            pOverlay->ChannelSize[1] = pOverlay->ChannelSize[0] / 2;
	        break;
	    // case COLOR_YCbCr420:
	    default: // COLOR_YCbCr420
	        pOverlay->ChannelSize[1] = pOverlay->ChannelSize[0] / 4;
	        break;
	}
	pOverlay->ChannelSize[2] = pOverlay->ChannelSize[1];

}

void *Display_GetBasePlane(DisplayContext *pCxt)
{
    return (void *)pCxt->vaBasePlane;
}

ECode Display_Create(DisplayContext *pCxt)
{
    SetDisplayDesc(pCxt);
    LcdParamInit(pCxt);
    if (!SetBaseplaneBuffer(pCxt, DRVBUF_SPEED_FAST)) return E_ACCESS_DENIED;
    return NOERROR;
}

ECode Display_Init(DisplayContext *pCxt)
{
    // Change frequency
    int speed = get_cpu_speed();
    kprintf("Before Change, CPU speed=%d\n", speed);
    ChangeFreq(cpu_freq_volt[3]);//to 312Mhz,run
    speed = get_cpu_speed();
    kprintf("After Change, CPU speed=%d\n", speed);

    // Before init, disable LCD controller
    LcdCtrl_Disable(pCxt);

    // Initialize the GPIO registers for proper LCD Controller operation
    Lcm_SetupGPIOs(pCxt);

    // Open back light
    BackLight_Open(pCxt);

    // Initialize the LCD Controller and frame descriptors
    LcdCtrl_Init(pCxt);

    // Enable the LCD controller
    LcdCtrl_Enable(pCxt);

    Lcm_DisplayOn(pCxt);

    ClearBaseplane(pCxt);

    return NOERROR;
}

ECode Display_SetOverlay(DisplayContext *pCxt, OverlayFormat *pOverlay)
{
    ECode ec;
    int layer = pOverlay->Layer;

    if (layer != 1 && layer != 2) return E_NOT_IMPLEMENTED;
    if (pCxt->overlay[layer].Enable) return E_ACCESS_DENIED; // Used by others

    // Now overlay1 & overlay2 can't be used at the same time
    // Overlay2's priority is higher
    if (layer == 2 && pCxt->overlay[1].Enable) {
        Display_DisableOverlay(pCxt, 1);
    }
    else if (layer == 1 && pCxt->overlay[2].Enable) {
        return E_ACCESS_DENIED;
    }

    // When use overlay, the color format of baseplane shoule be changed to RGBT555
    ec = Display_ChangeBasePlaneFormat(pCxt, COLOR_RGBT555);

    if (FAILED(ec)) return ec;

    // Allocate buffer for overlay.
    // Note 3 channels are all allocated for overlay2
    // when using YCbCr format.
    if (layer == 2 && (pOverlay->Color == COLOR_YCbCr422 || pOverlay->Color == COLOR_YCbCr420)) {
        pCxt->vaOverlay2Y = (unsigned int)Drv_AllocBuffer(
        OVERLAY2_FB_PAGES, 0);
        if (!pCxt->vaOverlay2Y) {
            kprintf("Alloc overlay2 buffer error!\n");
            ec = E_OUT_OF_MEMORY;
            goto Recover;
        }
        pCxt->paOverlay2Y =
            Drv_GetBufferPhysicalAddress((void *)pCxt->vaOverlay2Y);

        pCxt->paOverlay2Cb = pCxt->paOverlay2Y + OVERLAY2Y_FB_MAXSIZE;
        pCxt->paOverlay2Cr = pCxt->paOverlay2Cb + OVERLAY2Cb_FB_MAXSIZE;
        pCxt->vaOverlay2Cb = pCxt->vaOverlay2Y + OVERLAY2Y_FB_MAXSIZE;
        pCxt->vaOverlay2Cr = pCxt->vaOverlay2Cb + OVERLAY2Cb_FB_MAXSIZE;

        // Clear overlay2 framebuffer
        memset((void *)pCxt->vaOverlay2Y, 0, OVERLAY2Y_FB_MAXSIZE);
        memset((void *)pCxt->vaOverlay2Cb, 0x80, OVERLAY2Cb_FB_MAXSIZE);
        memset((void *)pCxt->vaOverlay2Cr, 0x80, OVERLAY2Cr_FB_MAXSIZE);
    }
    else {
        // Reserved for future use.
        ec = E_NOT_IMPLEMENTED;
        goto Recover;
    }

    ec = LcdCtrl_SetOverlay(pCxt, pOverlay);
    if (FAILED(ec)) goto Recover;

    memcpy(&pCxt->overlay[layer], pOverlay, sizeof(OverlayFormat));

    GetOverlayInfo(pCxt, layer);

    return NOERROR;

Recover:
    // The color format of baseplane shoule be changed to RGB565
    Display_ChangeBasePlaneFormat(pCxt, COLOR_RGB565);
    return ec;
}

void Display_DisableOverlay(DisplayContext *pCxt, int layer)
{
    OverlayFormat *pDrvOverlay;
    void *KernelBufAddr;
    int nPages;

    pDrvOverlay = &pCxt->overlay[layer];
    if (layer == 1) {
        // Future may use this!
//        KernelBufAddr = (void *)g_dispAddr.vaOverlay1;
//        nPages = ROUNDUP_PAGE((pDrvOverlay->Width * pDrvOverlay->Height
//            * pDrvOverlay->Bpp) >> 3) >> PAGE_SHIFT;
        return;
    }
    else if (layer == 2) {
        KernelBufAddr = (void *)pCxt->vaOverlay2Y;
        nPages = OVERLAY2_FB_PAGES;
    }
    else return;

    if(pDrvOverlay->Enable == FALSE) return;
    pDrvOverlay->Enable = FALSE;

    LcdCtrl_DisableOverlay(pCxt, layer);

    Drv_FreeBuffer(KernelBufAddr, nPages);

    // The color format of baseplane shoule be changed to RGB565
    Display_ChangeBasePlaneFormat(pCxt, COLOR_RGB565);
}

void Display_GetOverlayFormat(DisplayContext *pCxt, int layer,
    OverlayFormat *pOverlay)
{
    memcpy(pOverlay, &pCxt->overlay[layer], sizeof(OverlayFormat));
}

// Special use
ECode Display_ChangeBasePlaneFormat(DisplayContext *pCxt, ColorFormat format)
{
    if (pCxt->basePlaneFormat == COLOR_RGB565) {
        if (format == COLOR_RGBT555) {
            pCxt->basePlaneFormat = COLOR_RGBT555;
            goto CanSet;
        }
    }
    else if (pCxt->basePlaneFormat == COLOR_RGBT555) {
        if (format == COLOR_RGB565) {
            pCxt->basePlaneFormat = COLOR_RGB565;
            goto CanSet;
        }
    }

// CannotSet:
    return E_ACCESS_DENIED;

CanSet:
    LcdCtrl_Disable(pCxt);
    LcdCtrl_Init(pCxt);
    LcdCtrl_Enable(pCxt);
    return NOERROR;

}

//
// Power management
//

void Display_Suspend(DisplayContext *pCxt, int type, int lcdOff)
{
    if (lcdOff) {
        Lcm_DisplayOff(pCxt);
    }
    LcdCtrl_Suspend(pCxt, type);
}

void Display_Resume(DisplayContext *pCxt, int lcdOn)
{
    LcdCtrl_Resume(pCxt);
    if (lcdOn) {
        Lcm_DisplayOn(pCxt);
    }
}

